#include "mc_control.h"
#include "mc_math.h"

extern PidHandle_t spd_pid;


/**
  * @brief  low pass filter by coe input
  * @param  input: new value needed to be filter now
  * @param  last_out: value of last out, 
  * @param  coe: filter coefficience ,32767 means a float number of 1.0f
  *         and 16384 corrospand to 0.5f;
  * @retval output value this time
  */
int16_t low_pass_filter(int16_t input, int16_t last_out, uint16_t coe)
{
  return ((coe * input) >> 15) + (((32767 - coe) * last_out) >> 15);
}

/**
  * @brief  set H convertor's pwm duty value
  * @param  input: struct point of motor 
  * @retval err value
  */
uint16_t set_pwm(Motor_t *motor)
{
  uint16_t aux;
  uint16_t sa = motor->mc_foc_param.sa;
  uint16_t sb = motor->mc_foc_param.sb;
  uint16_t sc = motor->mc_foc_param.sc;
  if ((sa > (TIM_MAX_DUTY_CNT - TIM_DUTY_DEAD_CNT)) ||
      (sb > (TIM_MAX_DUTY_CNT - TIM_DUTY_DEAD_CNT)) ||
      (sc > (TIM_MAX_DUTY_CNT - TIM_DUTY_DEAD_CNT)))
  {
    return PWM_DUTY_OVERFLOW;
  }
  WRITE_REG(motor->mc_foc_param.foc_timmer->CCR1, sa);
  WRITE_REG(motor->mc_foc_param.foc_timmer->CCR2, sb);
  WRITE_REG(motor->mc_foc_param.foc_timmer->CCR3, sc);
  WRITE_REG(motor->mc_foc_param.foc_timmer->CCR4, motor->mc_foc_param.ch4_cnt);
  return 0;
}


int32_t speed_arr[SPEED_ARR_CNT] = {0}; // rad/s
// systick irq 1ms freq
/**
  * @brief  get rotor's speed, spd_rad is real speed with unit rad/s and spd_rad_norm
  *         is a spd_rad * 1024 as a  compatiable input of spd_pid, max spd_rad may less
  *         than 24rad/s = 1440rad/min
  * @param  input: struct point of motor 
  * @retval null
  */
void get_speed(Motor_t *motor)
{
  static int32_t m_angle_pre = 0;
  static int8_t speed_arr_index = 0;
  int32_t d_m_angle;
  int32_t m_angle = motor->encoder_content.encoder_count_pre[0];//count value 1~16384
  int32_t angle_diff = m_angle - m_angle_pre;//-16384 ~ 16384
  if (angle_diff > (CIRCLE_ENC_CNT/2)) // backward jump 0 to 360 degree
  {
    angle_diff = angle_diff - CIRCLE_ENC_CNT;
  }
  else if (angle_diff < -(CIRCLE_ENC_CNT/2))//forward jump from 360 to 0 degree
  {
    angle_diff = angle_diff + CIRCLE_ENC_CNT;
  }

  m_angle_pre = m_angle;//perserve the pre-value of encoder
  motor->encoder_content.speed_arr[speed_arr_index] = angle_diff;//perserve a difference value to array
  speed_arr_index++;
  if (speed_arr_index == (SPEED_ARR_CNT - 1))
  {
    speed_arr_index = 0;
  }
  int32_t sum_of_arr = 0;
  //get sum value of diff array
  for (uint8_t i = 0; i < SPEED_ARR_CNT; i++)
  {
    sum_of_arr += motor->encoder_content.speed_arr[i];
  }
  // sum_of_arr = sum_of_arr - speed_arr[0] + speed_arr[9],thsi mothed may used by FPGA
  //average cnt of 1ms, change unite to s by *1000, and get the mount of encoder puls change
  //of one circle by div 16384 (2^14).
  motor->encoder_content.speed_rad = ((sum_of_arr / SPEED_ARR_CNT) * 1000) >> 14;    // max 24rad/s = 1440rad/min,
  //get a speed_rad_nom value with a coe of 1024, rad_nom = rad * 1024 
  motor->encoder_content.speed_rad_nom = ((sum_of_arr / SPEED_ARR_CNT) * 1000) >> 4; // max 24*1024 =24576
}


/**
  * @brief  speed loop of foc control with pid regulator and release the id and iq from spd input
  * @param  input: struct point of motor 
  * @retval null
  */
void speed_loop_control(Motor_t *motor)
{
  Qd_t qd_ref_i = {0};
  int32_t spd_ref = motor->mc_foc_param.spd_ref;
  int32_t spd_rad_norm = motor->encoder_content.speed_rad_nom;
  int32_t spd_diff = -spd_rad_norm + spd_ref; //get diff
  qd_ref_i.d = 0;     //id is set as 0
  qd_ref_i.q = pi_controller(&spd_pid, spd_diff);//get new iq by trans spd_diff to pi controller
  motor->mc_foc_param.idq_ref.d = qd_ref_i.d; //get new id form spd_diff
  motor->mc_foc_param.idq_ref.q = qd_ref_i.q; //get new iq form spd_diff
}


/**
  * @brief  low freq task of systick nvic loop which be kicked up every 1ms
  *          it contans speed loop control
  * @param  input: struct point of motor 
  * @retval null
  */
void low_freq_task(Motor_t *motor)
{
  get_speed(motor);
  static uint8_t i = 0;
  i++;
  if (i == 1) // 1ms freq speed loop, change this value may expand speed loop cycle time
  {
    if (motor->mc_foc_param.control_mod == speed_mode)
    {
      //dynamic pid parameter of speed pid regulator
      if((motor->encoder_content.speed_rad_nom >= 500) ||(motor->encoder_content.speed_rad_nom <= -500) )
      {
        spd_pid.ki_gain = spd_pid.def_ki_gain;//5000, means 0.3f
      }
      else
      {
        spd_pid.ki_gain = spd_pid.def_ki_gain * 2;//10000, means 0.6f
      }
      speed_loop_control(motor);
    }
    i = 0;
  }
}



/**
  * @brief  get phase current of ia, ib, and through them to a low pass filter
  * @param  input: struct point of motor 
  * @retval null
  */
void get_tres_phase_current(Motor_t *motor)
{
  /* disable ADC trigger source */
  // CLEAR_BIT(TIM1->CCER, TIM_CCER_CC4E);// close trigger source until get needed current
  int16_t reg1 = -(2048 - motor->adc_content.adc1->JDR1 - 75);//max value 2048
  int16_t reg2 = -(2048 - motor->adc_content.adc2->JDR1 - 75);//max value 2048 corrospand to adc input voltage is 1.65V
  motor->mc_foc_param.ia = low_pass_filter(reg1, motor->mc_foc_param.ia, 10384);//coe = 10384/32767 = 0.32
  motor->mc_foc_param.ib = low_pass_filter(reg2, motor->mc_foc_param.ib, 10384);
  motor->mc_foc_param.ic = -motor->mc_foc_param.ia - motor->mc_foc_param.ib;
  //for the motor's high rotor resistor, the max current value is less than 1200 which is a short circle
  //current, so a coeficient 22 is need to expand max value ia ib to 32767
  //so ia_norm and ib_norm is a value between -32767 to 32767
  int32_t ia_norm = motor->mc_foc_param.ia * 22U;
  int32_t ib_norm = motor->mc_foc_param.ib * 22U;
  if(ia_norm > INT16_MAX)
  {
    ia_norm = INT16_MAX;
  }
  else if(ia_norm < INT16_MIN)
  {
    ia_norm = INT16_MIN;
  }
  if(ib_norm > INT16_MAX)
  {
    ib_norm = INT16_MAX;
  }
  else if(ib_norm < INT16_MIN)
  {
    ib_norm = INT16_MIN;
  }
  motor->mc_foc_param.ia_norm = (int16_t)ia_norm;
  motor->mc_foc_param.ib_norm = (int16_t)ib_norm;
  motor->mc_foc_param.ic_norm = -ia_norm - ib_norm;
}

/**
  * @brief  set adc sample point which is trigged by TIM1 CH4, so change
  *         TIM1->CCR4's value will shift the adc sample point
  * @param  input: struct point of motor 
  * @retval null
  */
void set_adc_samp_points(Motor_t *motor)
{
  uint16_t cnt_samp;
  cnt_samp = (uint32_t)(motor->mc_foc_param.half_duty) - 1u;
  motor->mc_foc_param.ch4_cnt = cnt_samp;//refresh tim_ch4 value
}


/**
 * @brief  It initializes the handle
 * @param  pHandle: handler of the current instance of the PID component
 * @retval None
 */
void pid_handle_init( PidHandle_t * pHandle )
{
  pHandle->kp_gain =  pHandle->def_kp_gain;
  pHandle->ki_gain =  pHandle->def_ki_gain;
  pHandle->kd_gain =  pHandle->def_kd_gain;
  pHandle->integral_term = 0x00000000UL;
  pHandle->prev_process_var_error = 0x00000000UL;
}


/**
 * @brief  This function compute the output of a PI regulator sum of its
 *         proportional and integral terms
 * @param  phandle: handler of the current instance of the PID component
 * @param  process_var_error: current process variable error, intended as the reference
 *         value minus the present process variable value
 * @retval computed PI output
 */
int16_t pi_controller(PidHandle_t *phandle, int32_t process_var_error)
{
    int32_t proportional_term, integral_term, output, integral_sum_temp;
    int32_t discharge = 0;
    int16_t upper_output_limit = phandle->upper_output_limit; // ID:32767   IQ:32767
    int16_t lower_output_limit = phandle->lower_output_limit; // ID:-32767  IQ:-32767
    /* Proportional term computation*/
    proportional_term = phandle->kp_gain * process_var_error; // ID = IQ: hKpGain = 3527

    /* Integral term computation */
    if (phandle->ki_gain == 0)
    {
        phandle->integral_term = 0;
    }
    else
    {
        integral_term = phandle->ki_gain * process_var_error; // ID = IQ: ki_gain = 307
        integral_sum_temp = phandle->integral_term + integral_term;

        if (integral_sum_temp < 0) // 溢出检测,由于有后面的限幅，此if else应当不会进入，upperLimit + maxWIntegral < 32767*16384 + 307 * 32767
        {
            if (phandle->integral_term > 0) // INT32 =  0 ~ 2,147,483,647 --> -2,147,483,648 ~ 0
            {
                if (integral_term > 0)
                {
                    integral_sum_temp = INT32_MAX;
                }
            }
        }
        else // 反向溢出
        {
            if (phandle->integral_term < 0)
            {
                if (integral_term < 0)
                {
                    integral_sum_temp = -INT32_MAX;
                }
            }
        }

        if (integral_sum_temp > phandle->upper_integral_limit) // UpperIntegralLimit = 32767 * 16384
        {
            phandle->integral_term = phandle->upper_integral_limit;
        }
        else if (integral_sum_temp < phandle->lower_integral_limit) // LowerIntegrallLimit = -32767 * 16384
        {
            phandle->integral_term = phandle->lower_integral_limit;
        }
        else
        {
            phandle->integral_term = integral_sum_temp;
        }
    }
    // hKpDivisorPOW2  id: 13 iq: 13 ;   hKiDivisorPOW2 id: 14 iq: 14
    // kp = 3527 ki = 307, float kp = 3527/(2^13) = 3527/81992 = 0.43; float ki = 307/(2^14) = 0.019
    // 第一部分 最大值 32767*0.43， 第二部分 32767*16384*0.019 所以求和后最大值是会超过int16
    output = (proportional_term >> phandle->kp_divisor_pow2) + (phandle->integral_term >> phandle->ki_divisor_pow2);
    if (output > upper_output_limit) // 32767
    {

        discharge = upper_output_limit - output; // 是负数，用来减少积分项，避免积分过大
        output = upper_output_limit;
    }
    else if (output < lower_output_limit) //-32767
    {

        discharge = lower_output_limit - output;
        output = lower_output_limit;
    }
    else
    { /* Nothing to do here */
    }
    phandle->integral_term += discharge;
    return ((int16_t)(output)); // 位数转换，由int32转为int16
}


/**
 * @brief  This function compute the output of a PID regulator sum of its
 *         proportional, integral and derivative terms
 * @param  pHandle: handler of the current instance of the PID component
 * @param  process_var_error: current process variable error, intended as the
 *         reference value minus the present process variable value
 * @retval PID computed output
 */
int16_t pid_controller(PidHandle_t *pHandle, int32_t process_var_error)
{
    int32_t differential_term;
    int32_t delta_error;
    int32_t temp_output;
    if (pHandle->kd_gain != 0) /* derivative terms not used */
    {
        delta_error = process_var_error - pHandle->prev_process_var_error;
        differential_term = pHandle->kd_gain * delta_error;
#ifdef FULL_MISRA_C_COMPLIANCY
        differential_term /= (int32_t)pHandle->hKdDivisor;
#else
        /* WARNING: the below instruction is not MISRA compliant, user should verify
        that Cortex-M3 assembly instruction ASR (arithmetic shift right)
        is used by the compiler to perform the shifts (instead of LSR
        logical shift right)*/
        differential_term >>= pHandle->kd_divisor_pow2;
#endif
        pHandle->prev_process_var_error = process_var_error;
        temp_output = pi_controller(pHandle, process_var_error) + differential_term;
        if (temp_output > pHandle->upper_output_limit)
        {
            temp_output = pHandle->upper_output_limit;
        }
        else if (temp_output < pHandle->lower_output_limit)
        {
            temp_output = pHandle->lower_output_limit;
        }
        else
        {
        }
    }
    else
    {
        temp_output = pi_controller(pHandle, process_var_error);
    }
    return ((int16_t)temp_output);
}

